#include "motionSeekerNode.h"


CMotionSeekerNode::CMotionSeekerNode (string name) : GSceneNode (name,"motionSeekerNode")
	{
		
		optFlow = new opticalFlow (1,2,256,256);
		
	m_oldCenter=128;
	m_trackingControl=0;
		
		
		m_serialAvailable=false;
		m_movingCamera=false;
		
		collunmShift=0;


		//m_velType1=0;
		m_motionControl=0;

		//m_velControl=2;

		

		if (initSerialInterface ())
		{
			m_serialAvailable=true;
			printf ("\nSerial Port Initialized!!!");
		}
		else
			printf ("\nCan t Initialize Serial Port!");

		/*setMoveSpeedControl (1);
		setMoveSpeedControl (8);*/

		//setMoveSpeedControl (2);
		//setMoveSpeedControl (8);

		setVelType (1);


		limite=37;

		plane[0].setPoints (-50,50,0);
		plane[1].setPoints (50,50,0);
		plane[2].setPoints (50,-50,0);
		plane[3].setPoints (-50,-50,0);

		
		planeFilter[0].setPoints (50,50,0);
		planeFilter[1].setPoints (150,50,0);
		planeFilter[2].setPoints (150,-50,0);
		planeFilter[3].setPoints (50,-50,0);


		planeGreyFilter[0].setPoints (-150,50,0);
		planeGreyFilter[1].setPoints (-50,50,0);
		planeGreyFilter[2].setPoints (-50,-50,0);
		planeGreyFilter[3].setPoints (-150,-50,0);

		motionSquare[0].setPoints (-150,50,1);
		motionSquare[1].setPoints (-50,50,1);
		motionSquare[2].setPoints (-50,-50,1);
		motionSquare[3].setPoints (-150,-50,1);


		

		device = setupESCAPI();
		if (device < 1)
		{
			printf ("\nUnable to init ESCAPI\n");
			
		}
		else
			for (int i = 0; i < device; i+=1)
			{
				char temp[256];
				getCaptureDeviceName(i, temp, 256);
				printf("\nDevice %d: \"%s\"\n", i, temp);
			}
		// Setup ESCAPI to capture a 256x256 image.

		movingDirec=90;
		int borderSize=50;
		border =new unsigned char[borderSize*256] ;
		oldBorder=new unsigned char[borderSize*256];
		oldScreen = new int[256*256];
		greyScaleScreen = new int[256*256];
		GreyScale = new unsigned char[256*256];
		OldGreyScale = new unsigned char[256*256];
		filterGreyScale= new unsigned char[256*256];
		filter = new int[256*256];
		filterGrey= new int [256*256];
		capture.mWidth = 256;
		capture.mHeight = 256;
		capture.mTargetBuf = new int[256*256];
		device=1;
		initCapture(device,&capture);
		doCapture(device);

		memcpy (oldScreen,capture.mTargetBuf, 256*256*sizeof (int));
		// OpenGL red and blue are swapped, so we'll need to do some converting here..
		for (int i = 0; i < 256*256; i+=1)
			capture.mTargetBuf[i] = (capture.mTargetBuf[i] & 0xff00ff00) |
			                        ((capture.mTargetBuf[i] & 0xff) << 16) |
									((capture.mTargetBuf[i] & 0xff0000) >> 16);



		


		// Create our texture
	glEnable(GL_TEXTURE_2D);
    
    glGenTextures(1,&texname);
    glBindTexture(GL_TEXTURE_2D,texname);

	// Load up some initial data
	glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)capture.mTargetBuf);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR); // Linear Filtering
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR); // Linear Filtering


	glGenTextures(1,&texnameFilter);
    glBindTexture(GL_TEXTURE_2D,texnameFilter);

	// Load up some initial data
	glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)filter);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR); // Linear Filtering
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR); // Linear Filtering


    glGenTextures(1,&	texnameFilterGrey);
    glBindTexture(GL_TEXTURE_2D,	texnameFilterGrey);
	glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)filterGrey);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR); // Linear Filtering
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR); // Linear Filtering


	

	glDisable(GL_TEXTURE_2D);




			for (int i=0; i<256; i+=1)
		{
			oldHistograma[i]=0;
			histograma[i]=0;
			
		}


		m_trackingControl=0;
		m_visible=true;

	
	}

	void CMotionSeekerNode::setVelType (int velType)
	{
		if (velType==0 | velType==1)
		{
					setMoveSpeedControl (2);
			setMoveSpeedControl (8);
		}
		if (velType==2 | velType==3)
		{
			setMoveSpeedControl (2);
			setMoveSpeedControl (8);
		}
		m_velType=velType;


	}

	void CMotionSeekerNode::refreshTexture ()
	{
		/*memcpy (oldScreen,capture.mTargetBuf, 256*256*sizeof (int));
		if (isCaptureDone(0))
		{
			// OpenGL red and blue are swapped, so we'll need to do some converting here..
			for (int i = 0; i < 256*256; i++)
				capture.mTargetBuf[i] = (capture.mTargetBuf[i] & 0xff00ff00) |
										((capture.mTargetBuf[i] & 0xff) << 16) |
										((capture.mTargetBuf[i] & 0xff0000) >> 16);
			
			// ..and ask for more
			doCapture(0);
		}

		for (int i=0; i<256*256; i+=1)
			filter[i]=capture.mTargetBuf[i]-oldScreen[i];*/

	}

	void CMotionSeekerNode::initInternal (bool creating)
	{


	}

	void CMotionSeekerNode::fromStringExternal (string stream)
	{
		if (stream.compare ("")==0)
			return;
		

	}

	string CMotionSeekerNode::toStringExternal ()
	{
		return "";
	}

	void CMotionSeekerNode::StaticCreation()
	{

	}

	void CMotionSeekerNode::DynamicDisplay()
	{
		controlCameraMovement ();
		//printf ("\nRender");
		{
			glEnable(GL_TEXTURE_2D);
			glBindTexture(GL_TEXTURE_2D,texname);
			//	glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)capture.mTargetBuf);
			glBegin (GL_QUADS);
			glNormal3f (0,0,1);
			glTexCoord2f(0, 0);
			glVertex3fv (plane[0].getData ());
			glTexCoord2f(0, 1);
			glVertex3fv (plane[1].getData ());
			glTexCoord2f(1, 1);
			glVertex3fv (plane[2].getData ());
			glTexCoord2f(1, 0);
			glVertex3fv (plane[3].getData ());
			glEnd ();

			if (isCaptureDone(device))
		{

			//aplicando filtro
			for (int i=0; i<256*256; i+=1)
			{
				 int a,b,c,d, l, e,f,g,h, aux;
				 l = oldScreen[i];
				 a= l & 0x000000FF;
				 b= (l & 0x0000FF00) >> 8;
				 c= (l & 0x00FF0000) >> 16;
				 d= (l & 0xFF000000) >> 24;

				 OldGreyScale[i]= (a+b+c)/3;

				 l=capture.mTargetBuf[i];
				 e= l & 0x000000FF;
				 f= (l & 0x0000FF00) >> 8;
				 g= (l & 0x00FF0000) >> 16;
				 h= (l & 0xFF000000) >> 24;

				 GreyScale[i]= (e+f+g)/3;
				 greyScaleScreen[i]=rgbaToInt (GreyScale[i],GreyScale[i],GreyScale[i],255);
				 

				 aux=0;
				 aux+= abs (255)<<24;
				 aux+= abs (c-g)<<16;
				 aux+= abs (b-f)<<8;
				 aux+= abs (a-e);
				 
				filter[i]=aux;
				filterGreyScale[i]=abs (GreyScale[i]-OldGreyScale[i]);

			}

			optFlow->calculate (OldGreyScale,GreyScale);
			//(int xInit,int yInit,int xSize,int ySize)
			

			getHistograma ();
			
			
			//matchImages ();
			trackingMovement();
			if (m_trackingControl==0)
			{
				//trackingMovement();
				moveCamera2 ();
			}
			else
			{
				//if (analyseHistograma(oldHistograma,histograma,256)<0.05f)
				{
					//matchImages ();
					
				}


				m_trackingControl-=1;
			}


			//adjustSquare (planeGreyFilter,motionSquare );

			/*float matrixGauss[9] = { 1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f};

			Convolution c;
			unsigned char* toDelete = GreyScale ;
			//GreyScale = c.applyConvolution (GreyScale,256,256,matrixGauss,3);
			GreyScale = c.derivativeX (GreyScale,256,256);
			delete toDelete;*/
			
			//for (int i=0; i<256*256; i+=1)
			//	filterGrey[i]=rgbaToInt ( GreyScale[i],GreyScale[i],GreyScale[i],255);

			//for (int i=0; i<256*256; i+=1)
			//	filterGrey[i]=rgbaToInt ( filterGreyScale[i],filterGreyScale[i],filterGreyScale[i],255);
			
//		
			memcpy (oldScreen, capture.mTargetBuf, 256*256*sizeof (int));

			// OpenGL red and blue are swapped, so we'll need to do some converting here..
			for (int i = 0; i < 256*256; i++)
				capture.mTargetBuf[i] = (capture.mTargetBuf[i] & 0xff00ff00) |
										((capture.mTargetBuf[i] & 0xff) << 16) |
										((capture.mTargetBuf[i] & 0xff0000) >> 16);
			

			// Load up the new data
			glBindTexture(GL_TEXTURE_2D,texname);
			glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)capture.mTargetBuf);
			
			// ..and ask for more
			doCapture(device);

		}

		

			glBindTexture(GL_TEXTURE_2D,texnameFilter);
			
				//glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)filter);
				glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)greyScaleScreen);


						glBegin (GL_QUADS);
			glNormal3f (0,0,1);
			glTexCoord2f(0, 0);
			glVertex3fv (planeFilter[0].getData ());
			glTexCoord2f(0, 1);
			glVertex3fv (planeFilter[1].getData ());
			glTexCoord2f(1, 1);
			glVertex3fv (planeFilter[2].getData ());
			glTexCoord2f(1, 0);
			glVertex3fv (planeFilter[3].getData ());
			glEnd ();

			glBindTexture(GL_TEXTURE_2D,texnameFilterGrey);
			glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,(GLvoid*)filterGrey);

						glBegin (GL_QUADS);
			glNormal3f (0,0,1);
			glTexCoord2f(0, 0);
			glVertex3fv (planeGreyFilter[0].getData ());
			glTexCoord2f(0, 1);
			glVertex3fv (planeGreyFilter[1].getData ());
			glTexCoord2f(1, 1);
			glVertex3fv (planeGreyFilter[2].getData ());
			glTexCoord2f(1, 0);
			glVertex3fv (planeGreyFilter[3].getData ());
			glEnd ();

			glDisable(GL_TEXTURE_2D);
			glDisable (GL_LIGHTING);
			
			vec3 motionSquare2[4];
			memcpy (motionSquare2,motionSquare,4*sizeof (vec3));
			adjustSquare (planeGreyFilter,motionSquare );
			//if (m_trackingControl==0)
			{
				glBegin (GL_LINE_LOOP);
				glColor3f (1,1,1);
				glVertex3fv (motionSquare[0].getData ());
				glVertex3fv (motionSquare[1].getData ());
				glVertex3fv (motionSquare[2].getData ());
				glVertex3fv (motionSquare[3].getData ());
				glEnd ();
			}

			adjustSquare (planeFilter,motionSquare2 );
			//if (m_trackingControl==0)
			{
				glBegin (GL_LINE_LOOP);
				glColor3f (1,1,1);
				glVertex3fv (motionSquare2[0].getData ());
				glVertex3fv (motionSquare2[1].getData ());
				glVertex3fv (motionSquare2[2].getData ());
				glVertex3fv (motionSquare2[3].getData ());
				glEnd ();
			}

			glBegin (GL_POINT);
			glPointSize (3);
			glVertex3f (-100,-150+(m_oldCenter*0.0039f)*100,2);
			glEnd ();

			

			drawHistograma (50,-50, 100, 100, histograma, 255);
			drawHistograma (50,-150, 100, 100, histogramaFiltro, 255);
			
			/*
			glColor3f (1,0,0);
			glBegin (GL_LINES);
			glVertex3f(50,-50+collunmShift*(0.4f),1);
			glVertex3f(-150,-50+collunmShift*(0.4f),1);
			glVertex3f(50,-50+collunmShift*(0.4f)+50*(0.4f),1);
			glVertex3f(-150,-50+collunmShift*(0.4f)+50*(0.4f),1);


			glEnd ();*/
			

			glEnable (GL_LIGHTING);
			optFlow->draw (50,-50,100,100);
			
				

		}

	}


	void CMotionSeekerNode::drawHistograma (float initX, float initY, float sizeX, float sizeY, int* histograma, int histogramSize)
	{
		int maxHistograma=-1;
		int i=0;
		for (i=0; i<histogramSize; i+=1)
			if (histograma[i]>maxHistograma)
				maxHistograma=histograma[i];


		float stepX = sizeX/histogramSize;
		float stepY = sizeY/maxHistograma;
		glLineWidth (2);
		glColor3f (0,0,0);
		glBegin (GL_LINE_STRIP);
		for (i=0; i<histogramSize; i+=1)
			//glVertex3f (initX+i*stepX, initY + histograma[i]*stepY,0);
			glVertex3f (initY+sizeY - histograma[i]*stepY,initX+i*stepX,0);
		glEnd ();
		glBegin (GL_LINE_LOOP);
		glVertex3f (initY,initX,0);
		glVertex3f (initY+sizeY,initX,0);
		glVertex3f (initY+sizeY,initX+sizeX,0);
		glVertex3f (initY,initX+sizeX,0);
			glEnd ();
			glLineWidth (1);
	}

	string CMotionSeekerNode::execCommandExt (scriptPackage *pkg)
{
	return "Command Invalid";
}

	string CMotionSeekerNode::ExecuteCommandExternal (string command)
	{


		stringAux toolbox;
		vector<string> vec_comm = toolbox.getFunction (command);
		
		if (vec_comm[0].compare ("False")!=0)
		{
			if (vec_comm.size()==1)
			{
				//nenhum parametro

				
				if (vec_comm[0].compare ("getVisible")==0)
					return toolbox.toStringBool (m_visible);

				//if (vec_comm[0].compare ("getLimite")==0)
				//	return toolbox.toStringInt (limite);
				
				if (vec_comm[0].compare ("getMoveSpeedControl")==0)
					return toolbox.toStringInt (m_velControl);

				if (vec_comm[0].compare ("moveCameraRight")==0)
				{
					moveCameraRight ();
					return "Moved Camera Right";
				}

				if (vec_comm[0].compare ("moveCameraLeft")==0)
				{
					moveCameraLeft ();
					return "Moved Camera Left";
				}
				
			}
			if (vec_comm.size()==2)
			{
		
				if (vec_comm[0].compare ("setVisible")==0)
				{
					m_visible=( toolbox.toBoolString (vec_comm[1]));
					return vec_comm[1];

				}
				//if (vec_comm[0].compare ("setLimite")==0)
				//{
				//	limite =  toolbox.toIntString (vec_comm[1]);
				//	return "True";
				//}
				if (vec_comm[0].compare ("setMoveSpeedControl")==0)
				{
					m_velControl = toolbox.toIntString (vec_comm[1]);
					return "True";
				}


			}
			
		}

		return ("Command Invalid");
	}

	void CMotionSeekerNode::mouseEventExternal (mouseEvt* evt)
	{
	}

	void CMotionSeekerNode::mouseDisableExternal ()
	{

	}

	 void CMotionSeekerNode::updateAABB ()
	 {
		 vec3 pMin, pMax;
		pMin.setMax ();
		pMax.setMin ();
		vec3 aux;

		//for (unsigned int i=0; i<data.size(); i+=1)
		{

		
		}
		m_aabbLocal.setMinMax (pMin, pMax);


	 }


	 void CMotionSeekerNode::print (int i)
	 {
		 int a,b,c,d;
		 a= i & 0x000000FF;
		 b= (i & 0x0000FF00) >> 8;
		 c= (i & 0x00FF0000) >> 16;
		 d= (i & 0xFF000000) >> 24;
		 printf ("\n a %d b %d c %d d %d",a,b,c,d);

	 }

	 int CMotionSeekerNode::rgbaToInt (unsigned char r,unsigned char g,unsigned char b,unsigned char a)
	 {
		 int i=0;
		 i+= a<<24;
		 i+= b<<16;
		 i+= g<<8;
		 i+= r;
		 return i;


	 }



	void CMotionSeekerNode::trackingMovement ()
	{
		
		bool  changed=false;
		int a=-1,b=-1;
		int c=500,d=500;
		for (int i=0; i<256; i+=1)
			for (int j=0; j<256; j+=1)
			{
				//printf ("\nL: %d",limite);
				if (filterGreyScale[i*256+j]>39)
				{
					changed=true;
					if (i>a)
						a=i;
					if (j>b)
						b=j;
					if (i<c)
						c=i;
					if (j<d)
						d=j;
				}
			}
		if (changed)
		{

			motionSquare[0].setPoints (c,b,1);
			motionSquare[1].setPoints (a,b,1);
			motionSquare[2].setPoints (a,d,1);
			motionSquare[3].setPoints (c,d,1);
		}
		else
		{
			motionSquare[0].setPoints (0,0,1);
			motionSquare[1].setPoints (0,0,1);
			motionSquare[2].setPoints (0,0,1);
			motionSquare[3].setPoints (0,0,1);
		}
		
	}

	
	void CMotionSeekerNode::adjustSquare (vec3* plane, vec3* square)
	{
		int xMin, xMax, yMin, yMax;
		xMin=xMax=(int)plane[0].getX();
		yMin=yMax=(int)plane[0].getY();
		for (int i=1; i<4;i+=1)
		{
			if (plane[i].getX ()>xMax)
				xMax=(int)plane[i].getX ();
			if (plane[i].getX ()<xMin)
				xMin=(int)plane[i].getX ();
			if (plane[i].getY ()>yMax)
				yMax=(int)plane[i].getY ();
			if (plane[i].getY ()<yMin)
				yMin=(int)plane[i].getY ();
		}


		float deltaX=(xMax-xMin)/256.0f;
		float deltaY=(yMax-yMin)/256.0f;

		for (int i=0; i<4; i+=1)
		{
			square[i].setX (xMin+square[i].getX()*deltaX   );
			square[i].setY (yMax- (square[i].getY()*deltaY)   );
		}
			

			

	}



	void CMotionSeekerNode::getHistograma ()
	 {
		for (int i=0; i<256; i+=1)
		{
			oldHistograma[i]=histograma[i];
			histograma[i]=0;
			histogramaFiltro[i]=0;
			
		}

		for (int i=0; i<256*256; i+=1)
		{
			histograma[ GreyScale[i]]+=1;
			histogramaFiltro[filterGreyScale[i]]+=1;
		}
	 }

	float CMotionSeekerNode::analyseHistograma (int *oldHistograma, int* newHistograma, int sizeHistograma)
	{
		float total=0;
		int diff;
		//int div=0;
		for (int i=0; i<sizeHistograma; i+=1)
		{
			diff = abs (newHistograma[i]-oldHistograma[i]);
			if (oldHistograma[i]!=0)	
			{
				total+= diff/oldHistograma[i];
				
				//div+=1;
			}
		}
		//if (div!=0)
			//total/=div;
		total/=sizeHistograma;
		//printf ("\n%f %%",total*100);

		return  total;


	}
	void CMotionSeekerNode::matchImages ()
	{
		int histogramaOriginal[255];
		int histogramaOld[255];

		printf ("\nMD %d",movingDirec);
		if (movingDirec!=0)
		{
			if (movingDirec==1)
			{

				//lado direito
				int imageSize=256;
				int pieceSize=50;
				int collumnInit=imageSize-pieceSize;
				unsigned char *p=GreyScale; 
				unsigned char *pB=border; 
				int idx=0;
				while (idx<=imageSize*imageSize)
				{
					p+=collumnInit;
					memcpy (pB,p,pieceSize);
					pB+=pieceSize;
					p+=pieceSize+ (imageSize-(collumnInit+pieceSize));
					idx+=imageSize;
				}

				for (int i=0; i<256; i+=1)
					histogramaOriginal[i]=0;
				for (int i=0; i<pieceSize*256; i+=1)
				{
					histogramaOriginal[border[i]]+=1; 
				}

				

				int halfImage = (int)(imageSize-imageSize*0.5f);
				
				int comparingTimes = imageSize-halfImage-pieceSize;
				float dif=65000;
				int collunm=-1;
				for (int i=0; i<comparingTimes; i+=1)
				{
					unsigned char *p=OldGreyScale; 
					unsigned char* oldB = oldBorder;
					idx=0;
					collumnInit=halfImage+i;
					while (idx<=imageSize*imageSize)
					{
						p+=collumnInit;
						memcpy (oldB,p,pieceSize);
						oldB+=pieceSize;
						p+=pieceSize+ (imageSize-(collumnInit+pieceSize));
						idx+=imageSize;			
					}


					for (int j=0; j<256; j+=1)
						histogramaOld[j]=0;
					for (int j=0; j<pieceSize*256; j+=1)
					{
						histogramaOld[oldBorder[j]]+=1; 
					}



					float difAux = analyseHistograma (histogramaOld, histogramaOriginal, 256);	
					if (difAux<dif)
					{
						dif=difAux;
						collunm=collumnInit;
					}
					

				}
				collunmShift=collunm;
				printf ("\nShift pra coluna direita %d %f ",collunm,dif);
			}
			/*
			else
			{
				//lado esquerdo
				int imageSize=256;
				int pieceSize=50;
				int collumnInit=pieceSize;
				unsigned char *p=GreyScale; 
				unsigned char *pB=border; 
				int idx=0;
				while (idx<=imageSize*imageSize)
				{
					p+=collumnInit;
					memcpy (pB,p,pieceSize);
					pB+=pieceSize;
					p+=pieceSize+ (imageSize-(collumnInit+pieceSize));
					idx+=imageSize;
				}

				for (int i=0; i<256; i+=1)
					histogramaOriginal[i]=0;
				for (int i=0; i<pieceSize*256; i+=1)
				{
					histogramaOriginal[border[i]]+=1; 
				}

				

				int halfImage = imageSize-imageSize*0.5f;
				
				int comparingTimes = imageSize-halfImage-pieceSize;
				float dif=65000;
				int collunm=-1;
				for (int i=0; i<comparingTimes; i+=1)
				{
					unsigned char *p=OldGreyScale; 
					unsigned char* oldB = oldBorder;
					idx=0;
					collumnInit=i;
					while (idx<=imageSize*imageSize)
					{
						p+=collumnInit;
						memcpy (oldB,p,pieceSize);
						oldB+=pieceSize;
						p+=pieceSize+ (imageSize-(collumnInit+pieceSize));
						idx+=imageSize;			
					}


					for (int j=0; j<256; j+=1)
						histogramaOld[j]=0;
					for (int j=0; j<pieceSize*256; j+=1)
					{
						histogramaOld[oldBorder[j]]+=1; 
					}



					float difAux = analyseHistograma (histogramaOld, histogramaOriginal, 256);	
					if (difAux<dif)
					{
						dif=difAux;
						collunm=collumnInit;
					}
					

				}
				collunmShift=collunm;
				printf ("\nShift pra coluna esquerda %d %f ",collunm,dif);

			}
			*/
		}





	}
 


	bool CMotionSeekerNode::initSerialInterface ()
	{
		//chupinhado de http://www.ontrak.net/mfc.htm
		BOOL     m_bPortReady;
		//HANDLE   m_hCom;
		//CString  m_sComPort;
		DCB      m_dcb;
		//COMMTIMEOUTS m_CommTimeouts;
		//BOOL     bReadRC;
		//DWORD iBytesRead;
		//char       sBuffer[128];

		//WCHAR m_sComPort[5] = {'C','O','M','1','\0'};
#if defined VS
		m_hCom = CreateFile(/*m_sComPort*/"COM1\0", 
				GENERIC_READ | GENERIC_WRITE,
				0, // exclusive access
				NULL, // no security
				OPEN_EXISTING,
				0, // no overlapped I/O
				NULL); // null template 
#else
			WCHAR m_sComPort[5] = {'C','O','M','1','\0'};
		m_hCom = CreateFile(m_sComPort, 
				GENERIC_READ | GENERIC_WRITE,
				0, // exclusive access
				NULL, // no security
				OPEN_EXISTING,
				0, // no overlapped I/O
				NULL); // null template 
#endif

		if (m_hCom==INVALID_HANDLE_VALUE)
			return false;

		m_bPortReady = SetupComm(m_hCom, 128, 128); // set buffer sizes

		//Port settings are specified in a Data Communication
		//Block (DCB). The easiest way to initialize a DCB is to
		//call GetCommState to fill in its default values, override 
		//the values that you want to change and then call SetCommState 
		//to set the values.

		m_bPortReady = GetCommState(m_hCom, &m_dcb);
		m_dcb.BaudRate = 9600;
		m_dcb.ByteSize = 8;
		m_dcb.Parity = NOPARITY;
		m_dcb.StopBits = ONESTOPBIT;
		m_dcb.fAbortOnError = FALSE;

		m_bPortReady = SetCommState(m_hCom, &m_dcb);

		/*
		//Communication timeouts are optional but can be set 
		//similarly to DCB values:

		m_bPortReady = GetCommTimeouts (m_hCom, &m_CommTimeouts);

		m_CommTimeouts.ReadIntervalTimeout = 50;
		m_CommTimeouts.ReadTotalTimeoutConstant = 50;
		m_CommTimeouts.ReadTotalTimeoutMultiplier = 10;
		m_CommTimeouts.WriteTotalTimeoutConstant = 50;
		m_CommTimeouts.WriteTotalTimeoutMultiplier = 10;

		m_bPortReady = SetCommTimeouts (m_hCom, &m_CommTimeouts);
*/


		return true;
	}
	
	bool CMotionSeekerNode::sendSerialMsg (char* message,int size)
	{
         BOOL     bWriteRC;

         DWORD iBytesWritten;

         bWriteRC = WriteFile(m_hCom, message,size,&iBytesWritten,NULL);
		
         
         return bWriteRC;
     }


	void CMotionSeekerNode::moveCameraRight ()
	{
		setMovingDirect (2);
		//sendSerialMsg ("D",1);
		//sendSerialMsg ("YII\0",4);
		//sendSerialMsg ("IIIIIIIIIIIIIIIIIIIIIIII",24);
		//sendSerialMsg ("L",1);
		//sendSerialMsg ("YI",2);


		if (m_velType==0)
		{
			sendSerialMsg ("D",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+2;
		}
		else
			if (m_velType==1)
		{
			sendSerialMsg ("D",1);
			m_motionControl=2;
			m_trackingControl=m_motionControl*3+1;
		}
			else if (m_velType==2)
			{
				sendSerialMsg ("R",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+4;

			}

		else if (m_velType==3)
			{
				sendSerialMsg ("Y",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+3;

			}
		
		
		m_movingCamera=true;
	}

		void CMotionSeekerNode::moveCameraRightFaster ()
	{
		sendSerialMsg ("D",1);
		//sendSerialMsg ("RIIIIIIIIIIIIIIIIIII",24);
		m_motionControl=4;
		m_trackingControl=m_motionControl*3+2;
		m_movingCamera=true;
	}


	void CMotionSeekerNode::moveCameraLeft ()
	{
		
		
		
		setMovingDirect (1);
		//sendSerialMsg ("TU",2);
		//sendSerialMsg ("K",1);
				//sendSerialMsg ("UUUUUUUUUUUUUUUUUUUUUUUU",24);
		if (m_velType==0)
		{
			sendSerialMsg ("A",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+2;
		}
		else
			if (m_velType==1)
		{
			sendSerialMsg ("A",1);
			m_motionControl=2;
			m_trackingControl=m_motionControl*3+1;
		}
			else if (m_velType==2)
			{
				sendSerialMsg ("E",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+4;

			}

						else if (m_velType==3)
			{
				sendSerialMsg ("T",1);
			m_motionControl=1;
			m_trackingControl=m_motionControl*3+3;

			}
		
			//m_trackingControl=m_motionControl*3+1;
		
		
		m_movingCamera=true;
	}



	void CMotionSeekerNode::moveCameraLeftFaster ()
	{
		sendSerialMsg ("A",1);
		//sendSerialMsg ("KH\0",4);
		//sendSerialMsg ("EUUUUUUUUUU",13);
		m_motionControl=4;
		m_trackingControl=m_motionControl*3+2;
		m_movingCamera=true;
	}

	void CMotionSeekerNode::controlCameraMovement ()
	{
		
		if (m_velType==0 | m_velType==1)
		{
			
			if (m_motionControl>0)
				m_motionControl-=1;
			else
			{
				sendSerialMsg ("S",1);
			}
		}
	}

	void CMotionSeekerNode::setMoveSpeedControl (int vel)
	{
		printf ("\nsetVel to %s",toolbox.toStringInt (vel).c_str());
		stringAux toolbox;
		sendSerialMsg ((char*)toolbox.toStringInt (vel).c_str(),strlen (toolbox.toStringInt (vel).c_str()));
		m_velControl=vel;
	}

	int CMotionSeekerNode::getMoveSpeedControl ()
	{
		return m_velControl;
	}

	void CMotionSeekerNode::moveCamera2 ()
	{
		int /*xMin,xMax,*/yMin,yMax,ySize;
		//xMin=motionSquare[0].getX();
		yMax=(int)motionSquare[0].getY();
		//xMax=motionSquare[1].getX();
		yMin=(int)motionSquare[3].getY();

		ySize= abs (yMax-yMin);
		int aux= ySize>>1;
		int centerY = yMin+aux;

		if ( ySize < 2)
		{
			m_oldCenter=128;
			return;
		}
		if ( (centerY < 133) & (centerY > 123))
		{
			//printf ("\nCENTRALIZADOOO!!!!!!");

			m_oldCenter=128;
			return;
		}

		if (m_oldCenter==128)
		{
			//printf ("\nReset");
			m_oldCenter=centerY;
		}
		else
		{
			//printf ("\nC %d",centerY);
			if (centerY>128)
			{
				moveCameraLeft ();
				//printf (" <- ");
				//if (centerY>m_oldCenter)
					//move fast
				//else
					//moveCameraLeft ();		
			}
			else
			{
				moveCameraRight ();
				//printf (" -> ");
				//if (centerY<m_oldCenter)
					//move fast??					
				//else
					//just move	
			}
		}
	}


	
